#include "gic_v2.h"
#include "register.h"

DEFINE_SYS_READ(s3_1_c15_c3_0)

DEFINE_SYS_READ(ID_PFR1_EL1)

static uint64_t PERIPH_BASE = 0;
static struct gic_map_reg GIC;


void gic_v2_clear(struct gic_map_reg *gic, uint32_t interruptId){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            gic->gicd->GICD_ICPENDRn[interruptId / 32] = (1 << (interruptId % 32));
        }
    }
}

void gic_v2_disable(struct gic_map_reg *gic, uint32_t interruptId){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            gic->gicd->GICD_ICENABLERn[interruptId / 32] = (1 << (interruptId % 32));
        }
    }
}

void gic_v2_enable(struct gic_map_reg *gic, uint32_t interruptId){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            gic->gicd->GICD_ISENABLERn[interruptId / 32] = (1 << (interruptId % 32));
        }
    }
}

void gic_v2_set_priority(struct gic_map_reg *gic, uint32_t interruptId, uint32_t priority){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            char* priority_addr = (char *) gic->gicd->GICD_IPRIORITYRn;
            priority_addr[interruptId] = (priority & 0xFF);
        }
    }
}

void gic_v2_set_core(struct gic_map_reg *gic, uint32_t interruptId, uint32_t core){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            char* core_addr = (char *) gic->gicd->GICD_ITARGETSRn;
            core_addr[interruptId] = (core & 0xFF);
        }
    }
}


void gic_v2_set_trigger_mode(struct gic_map_reg *gic,uint32_t interruptId, uint32_t mode){
    if(PERIPH_BASE){
        uint32_t max_spi = 32 * ((gic->gicd->GICD_TYPER & 0x1F) + 1);
        if(interruptId < max_spi){
            gic->gicd->GICD_ICFGRn[interruptId / 16] = (mode << ((interruptId % 16) * 2));
        }
    }
}

void interrupt_init(uint32_t interruptId, uint32_t mode){
    gic_v2_set_priority(&GIC, interruptId, 0);
    gic_v2_set_core(&GIC, interruptId, 0x1);
    gic_v2_clear(&GIC, interruptId);
    gic_v2_disable(&GIC, interruptId);
    gic_v2_enable(&GIC, interruptId);
    gic_v2_set_trigger_mode(&GIC, interruptId, mode & 0b11);
}




void gic_v2_init(){
    PERIPH_BASE = (sys_read_s3_1_c15_c3_0() >> 18 ) << 18;
    printk("periph_base : %x\n", PERIPH_BASE);

    uint64_t cpu_feature = sys_read_ID_PFR1_EL1();
    printk("cpu_feature: %x\n", cpu_feature);
    uint64_t gic_feature = (cpu_feature >> 28) & 0xF;
    if(gic_feature == 0){
        printk("GIC CPU interface system registers not implemented.");
    }
    else if(gic_feature == 1){
        printk("System register interface to versions 3.0 and 4.0 of the GIC CPU interface is supported.");
    }
    else if(gic_feature == 3){
        printk("System register interface to version 4.1 of the GIC CPU interface is supported.");
    }
    printk("\n");

    gic_distributor* gicd = (struct gic_distributor*)PERIPH_BASE;
    gic_cpu_interface* gicc = (struct gic_cpu_interface*)(PERIPH_BASE + 0x10000);
    GIC.gicc = gicc;
    GIC.gicd = gicd;

    uint32_t max_spi = 32 * ((GIC.gicd->GICD_TYPER & 0x1F) + 1);
    printk("max_spi: %d\n", max_spi);
    for(uint32_t i = 0; i < max_spi / 32; ++i){
        GIC.gicd->GICD_IGROUPRn[i] = 0;
        GIC.gicd->GICD_ISENABLERn[i] = 0;
        GIC.gicd->GICD_ICENABLERn[i] = 0xFFFFFFFF;
        GIC.gicd->GICD_ISPENDRn[i] = 0;
        GIC.gicd->GICD_ICPENDRn[i] = 0xFFFFFFFF;
        GIC.gicd->GICD_ISACTIVERn[i] = 0;
        GIC.gicd->GICD_ICACTIVERn[i] = 0xFFFFFFFF;
    }

    for(uint32_t i = 0; i < max_spi / 4; ++i){
        GIC.gicd->GICD_IPRIORITYRn[i] = 0xFFFFFFFF;
        GIC.gicd->GICD_ITARGETSRn[i] = 0;
    }

    for(uint32_t i = 0; i < max_spi / 16; ++i){
        GIC.gicd->GICD_ICFGRn[i] = 0xAAAAAAAA;
    }

    GIC.gicd->GICD_CTLR = 0x1;
    GIC.gicc->GICC_CTLR = 0x1;


    GIC.gicc->GICC_PMR = 0xFF;
    GIC.gicc->GICC_BPR = 0;



    printk("------  start print gic_map_reg  --------\n");
    printk("GIC->gicd: %x\n", GIC.gicd);
    printk("GIC->gicc: %x\n", GIC.gicc);

    printk("------  end print gic_map_reg  --------\n");
}



static uint32_t gic_v2_ack(struct gic_map_reg *gic){
    return gic->gicc->GICC_IAR;
}

static void gic_v2_eoi(struct gic_map_reg *gic, uint32_t interruptId){
    gic->gicc->GICC_EOIR = interruptId;
}



void gic_v2_handler(){
    uint32_t interruptId = gic_v2_ack(&GIC);

    switch (interruptId){
        case 30:
            generic_timer_handler();
            break;
        
        case 33:
            pl011_handler();
            break;
        
        default:
            break;
    }
    gic_v2_eoi(&GIC, interruptId);

}

